#include <opencv2/aruco.hpp>
#include <opencv2/opencv.hpp>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>

ros::Publisher pub_img;


cv::Ptr<cv::aruco::Dictionary> dict; // 编码词典 
// 根据序号,初始化dictionary参数指示板标记属于哪个标记字典。
std::vector<int> markerIds; // id序号  储存检测的marker对应的id号
std::vector< std::vector<cv::Point2f> > markerCorners, rejectedCandidates;
//第四个参数是类型的对象 DetectionParameters. 这一对象包含了检测阶段的所有参数

void pose_estimation(cv::Mat& inputImage)
{   
    dict = cv::aruco::getPredefinedDictionary(8);
    // 1 检测Marker标志，返回id和corner的列表
    // cv::aruco::detectMarkers(inputImage, dict, markerCorners, markerIds, parameters, rejectedCandidates);
    cv::aruco::detectMarkers(inputImage, dict, markerCorners, markerIds); 
    // std::cout << cam_conf_->cameraMatrix << "\n" << cam_conf_->distCoeffs << std::endl;
    if(markerIds.size()>0)
    {   
        ROS_INFO("detect successfully.");
        // 2 画结果
        cv::aruco::drawDetectedMarkers(inputImage, markerCorners, markerIds);
        // 3 估计位姿, 第二个参数markerLength边长,单位m
        // 检测的结果平移部分,相机坐标系下的向量,Z朝前  
        // cv::aruco::estimatePoseSingleMarkers(markerCorners, markerLength, cam_conf_->cameraMatrix, cam_conf_->distCoeffs, result.rvecs, result.tvecs);
        
        //绘制坐标轴，检查姿态估计结果
    }
}

void callback(const sensor_msgs::ImageConstPtr &msg)
{
    cv::Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
    pose_estimation(image);
    sensor_msgs::ImagePtr msg_img;
    msg_img = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
    pub_img.publish(msg_img);


}


int main(int argc, char** argv)
{   
    ros::init(argc, argv, "aruco_node");
    ros::NodeHandle n;

    pub_img = n.advertise<sensor_msgs::Image>("/camera/aruco", 10);
    ros::Subscriber sub_img = n.subscribe("/camera/color/image_raw", 10, &callback);
    ros::spin();
    return 0;
}


